//
// Created by xiang on 2022/7/18.
//

#include <gflags/gflags.h>
#include <glog/logging.h>

#include "ch7/loam-like/feature_extraction.h"
#include "common/io_utils.h"

#include "common/timer/timer.h"
#include "common/point_cloud_utils.h"

#include "tools/ui/pangolin_window.h"

/// 这里需要vlp16的数据，用wxb的
DEFINE_string(file_path, "/home/zl/Public/kaist_data/urban_data/", "数据文件路径");
DEFINE_string(txt_path, "/home/zl/Public/kaist_data/urban_data/sensor_data/data_stamp.csv", "数据文件路径");

int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    ui::PangolinWindow ui;
    if (ui.Init() == false) {
        return -1;
    }
    // 测试角点和平面点的提取
    sad::FeatureExtraction feature_extraction;

    system("rm -rf ./data/ch7/*.pcd");

    sad::KaistCsvIO io(FLAGS_file_path, FLAGS_txt_path);
    io.SetPointCloud3DProcessFunc([&](const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud_ptr) -> bool  {
            sad::CloudPtr pcd_corner(new sad::PointCloudType), pcd_surf(new sad::PointCloudType);
            common::Timer::Evaluate(
                [&]() {
                    feature_extraction.Extract(cloud_ptr, pcd_corner, pcd_surf);
                },
                "Feature Extraction");
            //LOG(INFO) << "original pts:" << cloud_ptr->size() << ", corners: " << pcd_corner->size()
            //                             << ", surf: " << pcd_surf->size();

            if(ui.ShouldQuit() == false) {
                ui.UpdateScan(pcd_corner, SE3());
                //ui.UpdateScan(pcd_surf, SE3());
            }

            //sad::SaveCloudToFile("./data/ch7/corner.pcd", *pcd_corner);
            //sad::SaveCloudToFile("./data/ch7/surf.pcd", *pcd_surf);
            return true;
    })
    .Go();

    common::Timer::PrintAll();
    LOG(INFO) << "done.";

    return 0;
}
